import RPi.GPIO as GPIO
import time
import cv2



def tonum(num):
    fm = 10.0 /180.0
    num = num *fm +2.5
    num = int(num*10)/10.0
    return num


pin_n1 = 18
pin_n2 = 23
pin_led = 12
pin_body = 25

GPIO.setmode(GPIO.BCM)
GPIO.setup(pin_n1, GPIO.OUT, initial=False)
GPIO.setup(pin_n2, GPIO.OUT, initial=False)
p1 = GPIO.PWM(pin_n1, 50)
p2 = GPIO.PWM(pin_n2, 50)
GPIO.setup(pin_led, GPIO.OUT)
# 初始化红外感应人体
GPIO.setup(pin_body, GPIO.IN)
GPIO.output(pin_led, GPIO.LOW)

p1.start(tonum(0))
p2.start(tonum(100))


time.sleep(0.5)

p1.ChangeDutyCycle(0)
p2.ChangeDutyCycle(0)

time.sleep(0.1)
recognizer = cv2.face.LBPHFaceRecognizer_create()  # 识别器
#recognizer.read('/Users/Shared/Previously Relocated Items/Security/Work/code_mrk/shumeipai/opencv/data/kratos.yml')  # 加载训练集
recognizer.read('/home/pi/kratos/test/shumeipai/opencv/data/kratos.yml')  # 加载训练集
#cascadePath = "/Users/Shared/Previously Relocated Items/Security/Work/code_mrk/shumeipai/opencv/haarcascade/haarcascade_frontalface_default.xml"
facecascadePath = "/home/pi/kratos/test/shumeipai/opencv/haarcascade/haarcascade_frontalface_default.xml"
# eyecascadePath = '/Users/Shared/Previously Relocated Items/Security/Work/code_mrk/shumeipai/opencv/haarcascade/haarcascade_eye.xml'

faceCascade = cv2.CascadeClassifier(facecascadePath)
# eyecascade = cv2.CascadeClassifier(eyecascadePath)

btm_count = 0
top_count = 0
id = 0
names = ['None', 'kratos', 'keivn']
topdegreeArray = [90,80,70,60,50]
btmdegreeArray = [10,20,30,40,50,60,70,80,90,100,110,120,130,140,150,160,170,180]


# 创建一个窗口 名字叫做Face
cv2.namedWindow('FaceDetect', flags=cv2.WINDOW_NORMAL | cv2.WINDOW_KEEPRATIO | cv2.WINDOW_GUI_EXPANDED)
cap = cv2.VideoCapture(0)
# 设置缓存区的大小 !!!
cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
print('IP摄像头是否开启： {}'.format(cap.isOpened()))
def face_filter(faces):
    '''
    对人脸进行一个过滤
    '''
    if len(faces) == 0:
        return None

    # 目前找的是画面中面积最大的人脸
    max_face = max(faces, key=lambda face: face[2] * face[3])
    (x, y, w, h) = max_face
    # face_area = img[y:y + h, x:x + w]  # 人脸区域
    # eyes = eyecascade.detectMultiScale(face_area, 1.1, 5)
    # # 用人眼级联分类器在人脸区域进行人眼识别，返回的eyes为眼睛坐标列表
    # for (ex, ey, ew, eh) in eyes:
    #     # 画出人眼框，绿色，画笔宽度为1
    #     cv2.rectangle(max_face, (ex, ey), (ex + ew, ey + eh), (0, 255, 0), 1)
    if w < 10 or h < 10:
        return None
    return max_face

def btm_steer(degree):
    '''
       设定云台底部舵机的角度
       '''

    btm_min_angle = 0  # 底部舵机最小旋转角度
    btm_max_angle = 180  # 底部舵机最大旋转角度

    if degree < btm_min_angle:
        degree = btm_min_angle
    elif degree > btm_max_angle:
        degree = btm_max_angle

    set_btm_degree(degrees=degree)

def set_btm_degree(degrees):
    p1.start(tonum(degrees))  #
    time.sleep(0.1)
    p1.ChangeDutyCycle(0)  # 清除当前占空比，使舵机停止抖动
    time.sleep(0.01)



def top_steer(degree):
    '''
       设定云台底部舵机的角度
       '''

    btm_min_angle = 0  # 底部舵机最小旋转角度
    btm_max_angle = 100  # 底部舵机最大旋转角度

    if degree < btm_min_angle:
        degree = btm_min_angle
    elif degree > btm_max_angle:
        degree = btm_max_angle

    set_top_degree(degrees=degree)

def set_top_degree(degrees):
    p2.start(tonum(degrees))  #
    time.sleep(0.1)
    p2.ChangeDutyCycle(0)  # 清除当前占空比，使舵机停止抖动
    time.sleep(0.01)


while True:
    if GPIO.input(pin_body):
        print("有人来了====================")
        ret, img = cap.read()
        # 手机画面水平翻转
        # img = cv2.flip(img, 1)
        # 将彩色图片转换为灰度图
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        # 检测画面中的人脸
        faces = faceCascade.detectMultiScale(
            gray,
            scaleFactor=1.2,
            minNeighbors=5
        )

        # 人脸过滤
        face = face_filter(faces)
        if face is not None:
            # 当前画面有人脸
            (x, y, w, h) = face
            # 在原彩图上绘制矩形
            cv2.rectangle(img, (x, y), (x + w, y + h), (0, 255, 0), 2)
            id, confidence = recognizer.predict(gray[y:y + h, x:x + w])
            img_height, img_width, _ = img.shape
            if (confidence < 100):
                id = names[id]
                # 点亮LED灯
                GPIO.output(pin_led, GPIO.HIGH)
                confidence = "  {0}%".format(round(100 - confidence))
                break
            else:
                id = "unknown"
                confidence = "  {0}%".format(round(100 - confidence))
        else:
            if btm_count == 17 and top_count == 4:
                print("有人靠近，但是没有检测到人脸")
                break
            elif btm_count < 17 and top_count < 4:
                time.sleep(1)
                btm_count += 1
                btm_steer(btmdegreeArray[btm_count])
            elif btm_count == 17 and top_count < 4:
                time.sleep(0.1)
                btm_count = 0
                top_count += 1
                btm_steer(0)

                top_steer(topdegreeArray[top_count])

            GPIO.output(pin_led, GPIO.LOW)

        # 在窗口Face上面展示图片img
        cv2.imshow('FaceDetect', img)
        # 等待键盘事件
        key = cv2.waitKey(1)
        if key == ord('q'):
            # 退出程序
            break

    else:
        print("周围没有人")
        time.sleep(5)

GPIO.cleanup()